#ifndef TELLO_H
// 这段头文件保护符的含义是，如果TELLO_H的值未被定义，则下列内容将被包含，若已被定义，下列内容无效
#define TELLO_H

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32.h"
#include "std_msgs/Empty.h"
#include "tf/transform_datatypes.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PoseArray.h"
#include <math.h>
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <Eigen/Core>

#define PI 3.1415926
using namespace std;

double t = 0, tstart = 0;
int flag = 0;
double K = -0.6; // PID控制比例参数

int tello_num = 4, obs_num = 0, ugvs_num = 4; // tello_num代表编队飞机的个数，obs_num代表障碍物的个数,ugvs_num代表小车的个数

double K_rep = 1.5;			 // 斥力场系数
double ro1 = 0.6, ro2 = 0.4; // 机间避障与障碍物避障的作用范围

int command = 0; // 控制指令参数

enum Command //iostream中的枚举类型
{
	STATE_INIT,
	TAKE_OFF,
	LAND,
	LAUNCH,
	STOP
};
Command command_;

struct Posture
{
	double x, y, z;
	double yaw, pitch, roll;
	double rtime_start, rtime_end;//仅给绕圈中心点使用
	double rolling_flag;
	double arrived;
};
Posture center; // 位置跟踪项

// struct Obstacle
// {
// 	double x, y, r;
// };
// std::vector<Obstacle *> obs_list;
geometry_msgs::PoseArray obs_list;

Eigen::Vector3d Quaterniond2Euler(const double x,const double y,const double z,const double w)
{
    Eigen::Quaterniond q;
    q.x() = x;
    q.y() = y;
    q.z() = z;
    q.w() = w;

    Eigen::Vector3d euler = q.toRotationMatrix().eulerAngles(2, 1, 0);
    // cout << "Quaterniond2Euler result is:" <<endl;
    // cout << "x = "<< euler[2] << endl ;
    // cout << "y = "<< euler[1] << endl ;
    // cout << "z = "<< euler[0] << endl << endl;
    return euler;
}

class Ugv
{
	string ID;

public:
	Posture pos;
	double vel_x,vel_y;
	ros::Subscriber pos_sub, vel_sub;
	// 输入无人车ID
	void setID(int i)
	{
		ID = to_string(i);
	}
	// 获取无人车ID
	string getID()
	{
		return ID;
	}
	void cb_readvel(const geometry_msgs::Twist::ConstPtr &msg)
	{
		vel_x = msg->linear.x;
		vel_y = msg->linear.x;
	}
	void cb_readpos(const geometry_msgs::PoseStamped::ConstPtr &msg)
	{
		// cout<<"收到小车位置,ID为:"<<ID<<endl;
		pos.x = msg->pose.position.x;
		pos.y = msg->pose.position.y;
	}
	// 实例化订阅者对象
	void initsubscriber(ros::NodeHandle nh)
	{
		string topic = "/ugv" + ID;
		pos_sub = nh.subscribe(topic + "/pose", 10, &Ugv::cb_readpos, this); // 这里必须加一个this来将对象成员绑定到当前实例
		vel_sub = nh.subscribe(topic + "twist", 10, &Ugv::cb_readvel, this);
	}
};
class Tello
{
	string ID; // tello的ID编号

public:
	Posture pos;										   // tello的位姿
	Posture H, V;										   // 编队的偏移量与速度补偿项
	Posture F, Fp;										   // 避碰补偿项
	double D[50], A[50], Fx[50], Fy[50], Fpx[50], Fpy[50]; // 机间距离，斥力函数等
	double dp_ob[50], ap_ob[50];						   // 用于储存飞行中心与障碍物之间的距离，以及二者之间的夹角
	geometry_msgs::Twist cmd_vel;						   // 速度控制量
	ros::Subscriber pos_sub;
	ros::Publisher takeoff_pub, vel_pub, land_pub;

	// 输入无人机ID
	void setID(int i)
	{
		ID = to_string(i);
	}
	// 获取无人机ID
	string getID()
	{
		return ID;
	}
	// 设计编队队形
	void setform(double x, double y, double vx, double vy);
	// 订阅无人机位置信息
	void cb_readpos(const geometry_msgs::PoseStamped::ConstPtr &msg);
	// 实例化订阅者对象
	void initsubscriber(ros::NodeHandle nh);
	// 实例化发布者对象
	void initpublisher(ros::NodeHandle nh);
	// 计算控制输入,并限制速度
	void vel_control1(Posture center,double n);
	// 跟车控制
	void vel_control2(std::vector<Ugv *> ugvs, double n);
	// 计算避障
	void vepul(std::vector<Tello *> uavs);
	//偏航角控制
	void yaw_control(double expect_yaw);
};

void Tello::setform(double x, double y, double vx, double vy)
{
	H.x = x, H.y = y;
	V.x = vx, V.y = vy;
}
void Tello::cb_readpos(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
	pos.x = msg->pose.position.x;
	pos.y = msg->pose.position.y;
	pos.z = msg->pose.position.z;

	// double Roll, Pitch, Yaw;
	// tf::Quaternion quat; // 将四元数转换为偏航角等信息
	// tf::quaternionMsgToTF(msg->pose.orientation, quat);
	// tf::Matrix3x3(quat).getRPY(Roll, Pitch, Yaw);
	// Yaw = Yaw * 180 / 3.14159;
	// Yaw = atan2(sin(Yaw), cos(Yaw));
	// pos.yaw = Yaw;
	Eigen::Vector3d euler;
	euler = Quaterniond2Euler(msg->pose.orientation.x,msg->pose.orientation.y,msg->pose.orientation.z,msg->pose.orientation.w);
	pos.yaw = euler[0];
}
void Tello::initsubscriber(ros::NodeHandle nh)
{
	// cout<<"收到tello位置,ID为:"<<ID<<endl;
	string topic = "/tello" + ID;
	pos_sub = nh.subscribe(topic + "/pose", 10, &Tello::cb_readpos, this); // 这里必须加一个this来将对象成员绑定到当前实例
}
void Tello::initpublisher(ros::NodeHandle nh)
{
	string topic = "/tello" + ID;
	// takeoff_pub = nh.advertise<std_msgs::Empty>(topic + "/takeoff", 10);
	// land_pub = nh.advertise<std_msgs::Empty>(topic + "/land", 10);
	vel_pub = nh.advertise<geometry_msgs::Twist>(topic + "/cmd", 10);
}
void Tello::vel_control1(Posture center, double n)
{
	cmd_vel.linear.x = K * (pos.x - center.x - H.x) + V.x + F.x + Fp.x;
	cmd_vel.linear.y = K * (pos.y - center.y - H.y) + V.y + F.y + Fp.y;
	if (abs(cmd_vel.linear.x) > n)
		cmd_vel.linear.x = (cmd_vel.linear.x > 0) ? n : -n;
	if (abs(cmd_vel.linear.y) > n)
		cmd_vel.linear.y = (cmd_vel.linear.y > 0) ? n : -n;
	// cout << cmd_vel.linear.x << " " << cmd_vel.linear.y << endl;
	vel_pub.publish(cmd_vel); 
}
void Tello::vel_control2(std::vector<Ugv *> ugvs, double n)
{
	int i = stoi(ID) - 1;
	// cout<<"我是"<<getID()<<"号tello"<<"我跟的是"<<i<<"号小车"<<endl;
	cmd_vel.linear.x = K * (pos.x - ugvs[i]->pos.x) + F.x + Fp.x;
	cmd_vel.linear.y = K * (pos.y - ugvs[i]->pos.y) + F.y + Fp.y;
	if (abs(cmd_vel.linear.x) > n)
		cmd_vel.linear.x = (cmd_vel.linear.x > 0) ? n : -n;
	if (abs(cmd_vel.linear.y) > n)
		cmd_vel.linear.y = (cmd_vel.linear.y > 0) ? n : -n;
	cout << ID <<"跟小车的距离为:" << pow(pos.x - ugvs[i]->pos.x,2) + pow(pos.y - ugvs[i]->pos.y,2) << endl;
}
void Tello::vepul(std::vector<Tello *> uavs)
{
	int i = 0; // 不要把这个变成全局变量
	// 初始化参数
	F.x = 0, F.y = 0;
	Fp.x = 0, Fp.y = 0;
	// 计算无人机机间的斥力
	for (i = 0; i < tello_num && i != stoi(ID) - 1; i++)
	{
		D[i] = sqrt(pow(uavs[i]->pos.x - pos.x, 2) + pow(uavs[i]->pos.y - pos.y, 2));
		A[i] = atan2(uavs[i]->pos.y - pos.y, uavs[i]->pos.x - pos.x);
	}
	for (i = 0; i < tello_num && i != stoi(ID) - 1; i++)
	{
		if (D[i] < ro1)
		{
			Fx[i] = -K_rep * (1 / D[i] - 1 / ro1) * (1 / D[i]) * cos(A[i]);
			Fy[i] = -K_rep * (1 / D[i] - 1 / ro1) * (1 / D[i]) * sin(A[i]);
			cout << ID << "与" << uavs[i]->getID() << "即将相撞" << endl;
			if (pos.x >= uavs[i]->pos.x)
				Fx[i] += 0.2; // 以0.2m/s的速度侧移离开障碍物范围
			else
				Fx[i] -= 0.2;
		}
		else
		{
			Fx[i] = 0;
			Fy[i] = 0;
		}
		F.x += Fx[i], F.y += Fy[i];
	}
	// 计算无人机与障碍物之间的斥力
	for (i = 0; i < obs_num; i++)
	{
		dp_ob[i] = sqrt(pow(pos.x - obs_list.poses[i].position.x, 2) + pow(pos.y - obs_list.poses[i].position.y, 2));
		ap_ob[i] = atan2(obs_list.poses[i].position.y - pos.y, obs_list.poses[i].position.x - pos.x);
		if (dp_ob[i] <= ro2 + obs_list.poses[i].position.z) // 如果进入到可能与障碍物碰撞的范围
		{
			cout << ID <<"与障碍物" << i << "即将相撞" <<endl;
			Fpx[i] = -K_rep * (1 / dp_ob[i] - 1 / (ro2 + obs_list.poses[i].position.z)) * (1 / (dp_ob[i] - obs_list.poses[i].position.z)) * cos(ap_ob[i]);
			Fpy[i] = -K_rep * (1 / dp_ob[i] - 1 / (ro2 + obs_list.poses[i].position.z)) * (1 / (dp_ob[i] - obs_list.poses[i].position.z)) * sin(ap_ob[i]);
			//如果出现进入障碍物范围被障碍物吸引的情况，可以考虑采用下面的代码
			// Fpx[i] = -K_rep * (1 / dp_ob[i] - 1 / (ro2 + obs_list.poses[i].position.z)) * (1 / (dp_ob[i])) * cos(ap_ob[i]);
			// Fpy[i] = -K_rep * (1 / dp_ob[i] - 1 / (ro2 + obs_list.poses[i].position.z)) * (1 / (dp_ob[i])) * sin(ap_ob[i]);
			//避免局部最优
			if (pos.x >= obs_list.poses[i].position.x)
				Fpx[i] += 0.3; // 以0.2m/s的速度侧移离开障碍物范围
			else
				Fpx[i] -= 0.3;
			if (pos.y >= obs_list.poses[i].position.y)
				Fpy[i] += 0.1; // 以0.1m/s的速度纵向离开障碍物范围
			else
				Fpy[i] -= 0.1;
		}
		else
		{
			Fpx[i] = 0;
			Fpy[i] = 0;
		}
		Fp.x += Fpx[i], Fp.y += Fpy[i];
	}
}

void cb_center(const geometry_msgs::PoseStamped::ConstPtr &msg) // 接收中心位置点的位置坐标
{
	geometry_msgs::PoseStamped p_cmd;
	p_cmd = *msg; // msg是数据整体，并不是一个指针
	center.x = p_cmd.pose.position.x;
	center.y = p_cmd.pose.position.y;
	center.rtime_start = p_cmd.pose.orientation.x;
	center.rtime_end = p_cmd.pose.orientation.y;
	center.rolling_flag = msg->pose.orientation.w;
	center.arrived = msg->pose.orientation.z;
}

void cb_command(const std_msgs::Int32::ConstPtr &msg) // 接收指令信息
{
	std_msgs::Int32 cmd_data;
	cmd_data = *msg;
	command = cmd_data.data;
	cout << "receive command: " << command << endl;
	if (command == 2 && flag == 0)
	{
		//时间记录只进行一次
		tstart = ros::Time::now().toSec();
		flag = 1;
	}
}

void cb_obstacle(const geometry_msgs::PoseArray::ConstPtr &msg)
{
	obs_list = *msg;
	obs_num = obs_list.poses.size();
}
#endif // TELLO_H